#ifndef _QUATERNION_H_
#define _QUATERNION_H_

#include "Utils\Types.h"
#include "Math\Matrix44.h"
#include "Math\Vector3.h"

class Quaternion
{
	public:

		//! Default Constructor
		Quaternion() : X(0.0f), Y(0.0f), Z(0.0f), W(1.0f) {}

		//! Constructor
		Quaternion(float32 x, float32 y, float32 z, float32 w) : X(x), Y(y), Z(z), W(w) { }

		//! Constructor which converts euler angles (radians) to a Quaternion
		Quaternion(float32 x, float32 y, float32 z);

		//! Constructor which converts euler angles (radians) to a Quaternion
		Quaternion(const Vect3f& vec);

		//! Equalilty operator
		bool operator==(const Quaternion& other) const;

		//! inequality operator
		bool operator!=(const Quaternion& other) const;

		//! Assignment operator
		inline Quaternion& operator=(const Quaternion& other);

		//! Add operator
		Quaternion operator+(const Quaternion& other) const;

		//! Multiplication operator
		Quaternion operator*(const Quaternion& other) const;

		//! Multiplication operator with scalar
		Quaternion operator*(float32 s) const;

		//! Multiplication operator with scalar
		Quaternion& operator*=(float32 s);

		//! Multiplication operator
		Vect3f operator*(const Vect3f& v) const;

		//! Multiplication operator
		Quaternion& operator*=(const Quaternion& other);

		//! Calculates the dot product
		inline float32 dotProduct(const Quaternion& other) const;

		//! Sets new Quaternion
		inline Quaternion& set(float32 x, float32 y, float32 z, float32 w);

		//! Sets new Quaternion based on euler angles (radians)
		inline Quaternion& set(float32 x, float32 y, float32 z);

		//! Sets new Quaternion based on euler angles (radians)
		inline Quaternion& set(const Vect3f& vec);

		//! Sets new Quaternion from other Quaternion
		inline Quaternion& set(const Quaternion& quat);

		//! returns if this Quaternion equals the other one, taking floating point rounding errors into account
		inline bool equals(const Quaternion& other, const float32 tolerance = 0.000001f ) const;

		//! Normalizes the Quaternion
		inline Quaternion& normalize();

		//! Creates a matrix from this Quaternion
		void getMatrix( Mat44f &dest, const Vect3f &translation=Vect3f() ) const;

		/*!
			Creates a matrix from this Quaternion
			Rotate about a center point
			shortcut for
			Quaternion q;
			q.rotationFromTo ( vin[i].Normal, forward );
			q.getMatrixCenter ( lookat, center, newPos );

			Mat44f m2;
			m2.setInverseTranslation ( center );
			lookat *= m2;

			Mat44f m3;
			m2.setTranslation ( newPos );
			lookat *= m3;

		*/
		void getMatrixCenter( Mat44f &dest, const Vect3f &center, const Vect3f &translation ) const;

		//! Creates a matrix from this Quaternion
		inline void getMatrix_transposed( Mat44f &dest ) const;

		//! Inverts this Quaternion
		Quaternion& makeInverse();

		//! Set this Quaternion to the linear interpolation between two Quaternions
		/** \param q1 First Quaternion to be interpolated.
		\param q2 Second Quaternion to be interpolated.
		\param time Progress of interpolation. For time=0 the result is
		q1, for time=1 the result is q2. Otherwise interpolation
		between q1 and q2.
		*/
		Quaternion& lerp(Quaternion q1, Quaternion q2, float32 time);

		//! Set this Quaternion to the result of the spherical interpolation between two Quaternions
		/** \param q1 First Quaternion to be interpolated.
		\param q2 Second Quaternion to be interpolated.
		\param time Progress of interpolation. For time=0 the result is
		q1, for time=1 the result is q2. Otherwise interpolation
		between q1 and q2.
		\param threshold To avoid inaccuracies at the end (time=1) the
		interpolation switches to linear interpolation at some point.
		This value defines how much of the remaining interpolation will
		be calculated with lerp. Everything from 1-threshold up will be
		linear interpolation.
		*/
		Quaternion& slerp(Quaternion q1, Quaternion q2,
				float32 time, float32 threshold=.05f);

		//! Create Quaternion from rotation angle and rotation axis.
		/** Axis must be unit length.
		The Quaternion representing the rotation is
		q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k).
		\param angle Rotation Angle in radians.
		\param axis Rotation axis. */
		Quaternion& fromAngleAxis (float32 angle, const Vect3f& axis);

		//! Fills an angle (radians) around an axis (unit vector)
		void toAngleAxis (float32 &angle, Vect3f& axis) const;

		//! Output this Quaternion to an euler angle (radians)
		void toEuler(Vect3f& euler) const;

		//! Set Quaternion to identity
		Quaternion& makeIdentity();

		//! Set Quaternion to represent a rotation from one vector to another.
		Quaternion& rotationFromTo(const Vect3f& from, const Vect3f& to);

		//! Quaternion elements.
		float32 X; // vectorial (imaginary) part
		float32 Y;
		float32 Z;
		float32 W; // real part
};


// Constructor which converts euler angles to a Quaternion
inline Quaternion::Quaternion(float32 x, float32 y, float32 z)
{
	set(x,y,z);
}


// Constructor which converts euler angles to a Quaternion
inline Quaternion::Quaternion(const Vect3f& vec)
{
	set(vec.x,vec.y,vec.z);
}

// equal operator
inline bool Quaternion::operator==(const Quaternion& other) const
{
	return ((X == other.X) &&
		(Y == other.Y) &&
		(Z == other.Z) &&
		(W == other.W));
}

// inequality operator
inline bool Quaternion::operator!=(const Quaternion& other) const
{
	return !(*this == other);
}

// assignment operator
inline Quaternion& Quaternion::operator=(const Quaternion& other)
{
	X = other.X;
	Y = other.Y;
	Z = other.Z;
	W = other.W;
	return *this;
}

// multiplication operator
inline Quaternion Quaternion::operator*(const Quaternion& other) const
{
	Quaternion tmp;

	tmp.W = (other.W * W) - (other.X * X) - (other.Y * Y) - (other.Z * Z);
	tmp.X = (other.W * X) + (other.X * W) + (other.Y * Z) - (other.Z * Y);
	tmp.Y = (other.W * Y) + (other.Y * W) + (other.Z * X) - (other.X * Z);
	tmp.Z = (other.W * Z) + (other.Z * W) + (other.X * Y) - (other.Y * X);

	return tmp;
}


// multiplication operator
inline Quaternion Quaternion::operator*(float32 s) const
{
	return Quaternion(s*X, s*Y, s*Z, s*W);
}


// multiplication operator
inline Quaternion& Quaternion::operator*=(float32 s)
{
	X*=s;
	Y*=s;
	Z*=s;
	W*=s;
	return *this;
}

// multiplication operator
inline Quaternion& Quaternion::operator*=(const Quaternion& other)
{
	return (*this = other * (*this));
}

// add operator
inline Quaternion Quaternion::operator+(const Quaternion& b) const
{
	return Quaternion(X+b.X, Y+b.Y, Z+b.Z, W+b.W);
}

/*!
	Creates a matrix from this Quaternion
*/
inline void Quaternion::getMatrix(Mat44f &dest,const Vect3f &center) const
{
	float32 m00 = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
	float32 m01 = 2.0f*X*Y + 2.0f*Z*W;
	float32 m02 = 2.0f*X*Z - 2.0f*Y*W;
	float32 m03 = 0.0f;

	float32 m10 = 2.0f*X*Y - 2.0f*Z*W;
	float32 m11 = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
	float32 m12 = 2.0f*Z*Y + 2.0f*X*W;
	float32 m13 = 0.0f;

	float32 m20 = 2.0f*X*Z + 2.0f*Y*W;
	float32 m21 = 2.0f*Z*Y - 2.0f*X*W;
	float32 m22  = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
	float32 m23  = 0.0f;

	float32 m30  = center.x;
	float32 m31  = center.y;
	float32 m32  = center.z;
	float32 m33  = 1.f;

	dest = Mat44f (m00, m01, m02, m03,
                   m10, m11, m12, m13,
                   m20, m21, m22, m23,
                   m30, m31, m32, m33);
}


// Inverts this Quaternion
inline Quaternion& Quaternion::makeInverse()
{
	X = -X; Y = -Y; Z = -Z;
	return *this;
}


// sets new Quaternion
inline Quaternion& Quaternion::set(float32 x, float32 y, float32 z, float32 w)
{
	X = x;
	Y = y;
	Z = z;
	W = w;
	return *this;
}


// sets new Quaternion based on euler angles
inline Quaternion& Quaternion::set(float32 x, float32 y, float32 z)
{
	float64 angle;

	angle = x * 0.5;
	const float64 sr = sin(angle);
	const float64 cr = cos(angle);

	angle = y * 0.5;
	const float64 sp = sin(angle);
	const float64 cp = cos(angle);

	angle = z * 0.5;
	const float64 sy = sin(angle);
	const float64 cy = cos(angle);

	const float64 cpcy = cp * cy;
	const float64 spcy = sp * cy;
	const float64 cpsy = cp * sy;
	const float64 spsy = sp * sy;

	X = (float32)(sr * cpcy - cr * spsy);
	Y = (float32)(cr * spcy + sr * cpsy);
	Z = (float32)(cr * cpsy - sr * spcy);
	W = (float32)(cr * cpcy + sr * spsy);

	return normalize();
}

// sets new Quaternion based on euler angles
inline Quaternion& Quaternion::set(const Vect3f& vec)
{
	return set(vec.x, vec.y, vec.z);
}

// sets new Quaternion based on other Quaternion
inline Quaternion& Quaternion::set(const Quaternion& quat)
{
	return (*this=quat);
}

// normalizes the Quaternion
inline Quaternion& Quaternion::normalize()
{
	const float32 n = X*X + Y*Y + Z*Z + W*W;

	if (n == 1)
	
return *this;

	//n = 1.0f / sqrtf(n);
//	return (*this *= reciprocal_squareroot ( n ));
}


// set this Quaternion to the result of the linear interpolation between two Quaternions
inline Quaternion& Quaternion::lerp(Quaternion q1, Quaternion q2, float32 time)
{
	const float32 scale = 1.0f - time;
	return (*this = (q1*scale) + (q2*time));
}

// calculates the dot product
inline float32 Quaternion::dotProduct(const Quaternion& q2) const
{
	return (X * q2.X) + (Y * q2.Y) + (Z * q2.Z) + (W * q2.W);
}


//! axis must be unit length, angle in radians
inline Quaternion& Quaternion::fromAngleAxis(float32 angle, const Vect3f& axis)
{
	const float32 fHalfAngle = 0.5f*angle;
	const float32 fSin = sinf(fHalfAngle);
	W = cosf(fHalfAngle);
	X = fSin*axis.x;
	Y = fSin*axis.y;
	Z = fSin*axis.z;
	return *this;
}

inline Vect3f Quaternion::operator* (const Vect3f& v) const
{
	// nVidia SDK implementation

	Vect3f uv, uuv;
	Vect3f qvec(X, Y, Z);
	uv = qvec.CrossProduct(v);
	uuv = qvec.CrossProduct(uv);
	uv *= (2.0f * W);
	uuv *= 2.0f;

	return v + uv + uuv;
}

// set Quaternion to identity
inline Quaternion& Quaternion::makeIdentity()
{
	W = 1.f;
	X = 0.f;
	Y = 0.f;
	Z = 0.f;
	return *this;
}

#endif

